import os
import rosnode

prefix = 'bash -c "source {}/env.sh && '.format(os.path.expanduser('~'))

nodes = {
  '/ubiquityrobot/camera_node': {
    'name': 'camera_node',
    'text': '摄像头',
    'cmd': 'roslaunch pi_cam camera_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/apriltag_detector_node': {
    'name': 'apriltag_detector_node',
    'text': '标签检测',
    'cmd': 'roslaunch pi_cam apriltag_detector_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/line_detector_node': {
    'name': 'line_detector_node',
    'text': '颜色检测',
    'cmd': 'roslaunch pi_cam line_detector_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/face_recognizer_node': {
    'name': 'face_recognizer_node',
    'text': '人脸识别',
    'cmd': 'roslaunch pi_ai face_recognizer_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/barcode_scanner_node': {
    'name': 'barcode_scanner_node',
    'text': '二维码扫描',
    'cmd': 'roslaunch pi_cam barcode_scanner_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/text_recognizer_node': {
    'name': 'text_recognizer_node',
    'text': '文本识别',
    'cmd': 'roslaunch pi_ai text_recognizer_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/image_processor_node': {
    'name': 'image_processor_node',
    'text': '图像处理',
    'cmd': 'roslaunch pi_cam image_processor_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/smart_audio_node': {
    'name': 'smart_audio_node',
    'text': '智能语音',
    'cmd': 'DISPLAY=:0 roslaunch pi_ai smart_audio_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/object_detector_node': {
    'name': 'object_detector_node',
    'text': '目标检测',
    'cmd': 'roslaunch pi_ai object_detector_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/image_classifier_node': {
    'name': 'image_classifier_node',
    'text': '图像分类',
    'cmd': 'roslaunch pi_ai image_classifier_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/hand_detector_node': {
    'name': 'hand_detector_node',
    'text': '手势识别',
    'cmd': 'roslaunch pi_ai hand_detector_node.launch" ',
    'auto_start': True
  },
  '/ubiquityrobot/pose_estimator_node': {
    'name': 'pose_estimator_node',
    'text': '姿态估计',
    'cmd': 'roslaunch pi_ai pose_estimator_node.launch" ',
    'auto_start': True
  },
  # '/ubiquityrobot/movenet_pose_node': {
  #   'name': 'movenet_pose_node',
  #   'text': '姿态估计(快速)',
  #   'cmd': 'roslaunch pi_ai movenet_pose_node.launch" ',
  #   'auto_start': True
  # },
  '/ubiquityrobot/rfid_node': {
    'name': 'rfid_node',
    'text': 'RFID读卡器',
    'cmd': 'roslaunch pi_driver rfid_node.launch" ',
    'auto_start': False
  },
  '/ubiquityrobot/ble_ros_node': {
    'name': 'ble_ros_node',
    'text': 'RFID读卡器',
    'cmd': 'roslaunch pi_driver ble_ros_node.launch" ',
    'auto_start': False
  },
  '/ubiquityrobot/joystick_node': {
    'name': 'joystick_node',
    'text': '游戏手柄',
    'cmd': 'roslaunch pi_driver joystick_node.launch" ',
    'auto_start': False
  },
  '/ubiquityrobot/brushless_driver_node': {
    'name': 'brushless_driver_node',
    'text': '平衡车',
    'cmd': 'roslaunch pi_robot brushless_driver_node.launch" ',
    'auto_start': False
  },
  '/ubiquityrobot/pupper_driver_node': {
    'name': 'pupper_driver_node',
    'text': '四足机器人',
    'cmd': 'roslaunch pi_robot pupper_driver_node.launch" ',
    'auto_start': False
  },
  '/ubiquityrobot/hexapod_driver_node': {
    'name': 'hexapod_driver_node',
    'text': '六足机器人',
    'cmd': 'roslaunch pi_robot hexapod_driver_node.launch" ',
    'auto_start': False
  },

}

def get_node_list():
    nodes_list = rosnode.get_node_names()
    for k,v in nodes.items():
        if k in nodes_list:
            v["status"] = '已启动'
        else:
            v["status"] = '已停止'
        v["value"] = k
    return nodes

def start_node(name):
    nodes_list = rosnode.get_node_names()
    if name in nodes and name not in nodes_list:
        os.system(prefix+nodes[name]['cmd']+' > /tmp/{}.log &'.format(nodes[name]['name']))

def stop_node(name):
    nodes_list = rosnode.get_node_names()
    if name in nodes_list:
        rosnode.kill_nodes([name])

if __name__ == '__main__':
    import time
    # print(get_node_list())
    # start_node('/ubiquityrobot/apriltag_detector_node')
    # time.sleep(10)
    stop_node('/ubiquityrobot/apriltag_detector_node')
